#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <GeographicLib/UTMUPS.hpp>
#include "robot_interfaces/msg/zone_northp.hpp"

class GpsToUtmOdomNode : public rclcpp::Node
{
public:
    GpsToUtmOdomNode()
        : Node("gps_to_utm_odometry_node"),
          last_easting_(0.0), last_northing_(0.0), last_stamp_(0)
    {
        this->declare_parameter<std::string>("gps_topic", "/fix/filter"); // GPS数据话题
        this->declare_parameter<std::string>("odom_topic", "utm/gps");    // 输出的UTM里程计话题
        this->declare_parameter<std::string>("zone_northp_topic", "utm/zone/northp");    // UTM分区和北半球标志话题
        this->declare_parameter<std::string>("frame_id", "utm");          // 坐标系ID
        this->declare_parameter<std::string>("child_frame_id", "gps");    // 子坐标系ID
        this->declare_parameter<bool>("use_velocity", false);             // 是否进行航向推算
        this->declare_parameter<bool>("use_yaw", false);                  // 是否进行速度推算

        auto gps_topic = this->get_parameter("gps_topic").as_string();
        auto odom_topic = this->get_parameter("odom_topic").as_string();
        auto zone_northp_topic = this->get_parameter("zone_northp_topic").as_string();
        frame_id_ = this->get_parameter("frame_id").as_string();
        child_frame_id_ = this->get_parameter("child_frame_id").as_string();
        use_velocity_ = this->get_parameter("use_velocity").as_bool();
        use_yaw_ = this->get_parameter("use_yaw").as_bool();

        gps_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            gps_topic, rclcpp::SensorDataQoS(),
            std::bind(&GpsToUtmOdomNode::gps_callback, this, std::placeholders::_1));
        odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>(odom_topic, 10);
        zone_northp_pub_ = this->create_publisher<robot_interfaces::msg::ZoneNorthp>(zone_northp_topic, 10);

        RCLCPP_INFO(this->get_logger(), "GPS到UTM坐标转换节点已启动，订阅:%s 发布:%s", gps_topic.c_str(), odom_topic.c_str());
    }

private:
    void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
    {
        // 只处理有效GPS数据
        if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX)
        {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "GPS无效，跳过此帧。");
            return;
        }

        nav_msgs::msg::Odometry odom_msg;
        odom_msg.header.stamp = msg->header.stamp;
        odom_msg.header.frame_id = frame_id_;
        odom_msg.child_frame_id = child_frame_id_;

        double northing, easting;
        int zone;
        bool northp;
        try
        {
            GeographicLib::UTMUPS::Forward(
                msg->latitude, msg->longitude, zone, northp, easting, northing);

            odom_msg.pose.pose.position.x = easting;
            odom_msg.pose.pose.position.y = northing;
            odom_msg.pose.pose.position.z = msg->altitude;
            rclcpp::Time now = msg->header.stamp;

            // 方向推断：用前后两帧简单推算
            if (use_velocity_)
            {
                double vx = 0.0, vy = 0.0;
                if (last_stamp_.nanoseconds() > 0)
                {
                    double dt = (now - last_stamp_).seconds();
                    if (dt > 1e-3)
                    {
                        vx = (easting - last_easting_) / dt;
                        vy = (northing - last_northing_) / dt;
                    }
                }
                odom_msg.twist.twist.linear.x = vx;
                odom_msg.twist.twist.linear.y = vy;
            }

            // 航向角估算
            if (use_yaw_)
            {
                // 计算航向角
                double yaw = 0.0;
                if (last_stamp_.nanoseconds() > 0)
                {
                    double dx = easting - last_easting_;
                    double dy = northing - last_northing_;
                    if (dx != 0.0 || dy != 0.0)
                    {
                        yaw = std::atan2(dy, dx);
                    }
                }
                odom_msg.pose.pose.orientation.x = 0.0;
                odom_msg.pose.pose.orientation.y = 0.0;
                odom_msg.pose.pose.orientation.z = std::sin(yaw * 0.5);
                odom_msg.pose.pose.orientation.w = std::cos(yaw * 0.5);
            }
            else
            {
                // 如果不使用yaw，则设置为默认值
                odom_msg.pose.pose.orientation.x = 0.0;
                odom_msg.pose.pose.orientation.y = 0.0;
                odom_msg.pose.pose.orientation.z = 0.0;
                odom_msg.pose.pose.orientation.w = 1.0;
            }

            // 协方差复制
            // for (size_t i = 0; i < 36; ++i)
            // {
            //     odom_msg.pose.covariance[i] = 0.0;
            // }
            // odom_msg.pose.covariance[0] = std::max(1e-3, msg->position_covariance[0]);
            // odom_msg.pose.covariance[7] = std::max(1e-3, msg->position_covariance[4]);
            // odom_msg.pose.covariance[14] = std::max(1e-3, msg->position_covariance[8]);

            // 速度协方差也可简单设置
            // odom_msg.twist.covariance[0] = 0.2;
            // odom_msg.twist.covariance[7] = 0.2;
            // odom_msg.twist.covariance[14] = 0.2;

            // 增加zone信息，可在frame_id或child_frame_id中体现，或发布diagnostic
            odom_msg.header.frame_id = frame_id_ + "_zone" + std::to_string(zone) + (northp ? "N" : "S");

            odom_pub_->publish(odom_msg);
            publishZoneNorthp(zone, northp);
            // 保存历史
            last_easting_ = easting;
            last_northing_ = northing;
            last_stamp_ = now;

            // RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
            //                      "GPS转换UTM: zone %d %s (%.2f, %.2f), yaw: %.2f deg, vx: %.2f m/s, vy: %.2f m/s",
            //                      zone, northp ? "N" : "S", easting, northing, yaw * 180.0 / M_PI, vx, vy);

            RCLCPP_INFO(this->get_logger(), "GPS转换UTM: zone %d %s (%.2f, %.2f), yaw: %.2f deg",
                        zone, northp ? "N" : "S", easting, northing, 0.0);
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "UTM转换失败: %s", e.what());
        }
    }
    void publishZoneNorthp(int zone, bool northp)
    {
        auto zone_northp_msg = robot_interfaces::msg::ZoneNorthp();
        zone_northp_msg.zone = zone;
        zone_northp_msg.northp = northp;
        zone_northp_pub_->publish(zone_northp_msg);
    }
    std::string frame_id_, child_frame_id_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
    rclcpp::Publisher<robot_interfaces::msg::ZoneNorthp>::SharedPtr zone_northp_pub_;

    // 用于速度和航向估算
    double last_easting_, last_northing_;
    rclcpp::Time last_stamp_;
    bool use_velocity_, use_yaw_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GpsToUtmOdomNode>());
    rclcpp::shutdown();
    return 0;
}